/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2013, PAL Robotics, S.L.
 *  All rights reserved.
 *

 *********************************************************************/

#include <chassis_controller/odometry_multi_pivots.h>
#include <boost/bind.hpp>



namespace chassis_controller
{
  MultiPivotsOdometry::MultiPivotsOdometry(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node):Odometry(node)
  {
     timestamp_ = node_->now();
  }

  std::vector<Sxya> delta_pivot_pos;
  std::vector<Sxya> delta_pos_in_vehicle;
  std::vector<Sxya> pivot_vel_in_vehicle;

  void MultiPivotsOdometry::init(const rclcpp::Time &time, std::vector<Spivot_value> &pivot_value)
  {
      // Reset accumulators and timestamp:
      timestamp_ = time;
      // init wheel pos
      int pivot_num = pivot_value.size();
      for (int i = 0; i < pivot_num; i++)
      {
      }
  }

  void MultiPivotsOdometry::update(std::vector<Spivot_value> &pivot_value, std::vector<Spivot_param> &pivot_param , const rclcpp::Time &time , bool standstill,double odom_factor)
  {
    int pivot_num = pivot_value.size();
    pivot_vel_.resize(pivot_num);
    pivot_pos_.resize(pivot_num);
    previous_pivot_pos_.resize(pivot_num);
    delta_pivot_pos.resize(pivot_num);
    delta_pos_in_vehicle.resize(pivot_num);
    pivot_vel_in_vehicle.resize(pivot_num);

    //step1: calculate velocity and position of pivot
    for (int i =0 ;i<pivot_num ;i++)
    {
      if (pivot_param[i].pivot_type == DIFFWHEEL)
      { 
        pivot_value[i].velocity = (pivot_value[i].joint_value[0].velocity + pivot_value[i].joint_value[1].velocity)/2.0;        
        pivot_value[i].position = (pivot_value[i].joint_value[0].position + pivot_value[i].joint_value[1].position)/2.0;
        pivot_value[i].steer_position = pivot_value[i].external_angle;       
      }
      else if (pivot_param[i].pivot_type == STEERWHEEL)
      {
        pivot_value[i].steer_position = pivot_value[i].steer_position;
        if (pivot_param[i].use_external_encoder)
        pivot_value[i].steer_position = pivot_value[i].external_angle;
      }
      DEBUG_OUT_THROTTLE(get_clock(),
        500,"[odom_multi_pivot] pivot" <<i << " position : " << pivot_value[i].position);
      DEBUG_OUT_THROTTLE(get_clock(),
        500,"[odom_multi_pivot] pivot" <<i << " velocity : " << pivot_value[i].velocity);
      DEBUG_OUT_THROTTLE(get_clock(),
        500,"[odom_multi_pivot] pivot" <<i << " steer position : " << pivot_value[i].steer_position);
      //calculate velocity of pivot in vehicle coordinate
      pivot_vel_[i].x = pivot_value[i].velocity;      
      pivot_vel_[i].y = 0.0;
      pivot_vel_[i].a = 0.0;
      pivot_vel_in_vehicle[i] = frameTrans(pivot_vel_[i] , pivot_value[i].steer_position);
      //calculate delta position of pivot in vehicle coordinate
      pivot_pos_[i] = pivot_value[i].position;
      delta_pivot_pos[i].x = (pivot_pos_[i] - previous_pivot_pos_[i])*odom_factor;
      if (abs(delta_pivot_pos[i].x)>1)
      {delta_pivot_pos[i].x = 0.0; }     
      delta_pivot_pos[i].y = 0.0;
      delta_pivot_pos[i].a = 0.0; 
      delta_pos_in_vehicle[i] = frameTrans(delta_pivot_pos[i] , pivot_value[i].steer_position);
      previous_pivot_pos_[i] = pivot_pos_[i];
      // RCLCPP_INFO_STREAM("pivot" << i << "delta_pivot_pos x,y,a: " << delta_pivot_pos[i].x << ","
      //                           << delta_pivot_pos[i].y << "," << delta_pivot_pos[i].a );      
      // RCLCPP_INFO_STREAM("pivot" << i << "delta_pos_in_vehicle : " << delta_pos_in_vehicle[i].x << ","
      //                           << delta_pos_in_vehicle[i].y << "," << delta_pos_in_vehicle[i].a );
    }

    //step2: calculate delta theta of vehicle and angular velocity
    double delta_offset_x , delta_offset_y , delta_theta , omiga;
    delta_theta = omiga = 0.0;
    for (int i =1 ;i<pivot_num ;i++)
    {      
      if (abs(pivot_param[i].offset_x-pivot_param[i-1].offset_x) >= 
        abs(pivot_param[i].offset_y-pivot_param[i-1].offset_y))
      {
        delta_offset_x = (pivot_param[i].offset_x-pivot_param[i-1].offset_x);
        delta_theta += (delta_pos_in_vehicle[i].y - delta_pos_in_vehicle[i-1].y  )/delta_offset_x;
        omiga += (pivot_vel_in_vehicle[i].y - pivot_vel_in_vehicle[i-1].y) / delta_offset_x;
      }
      else 
      {
        delta_offset_y = (pivot_param[i].offset_y-pivot_param[i-1].offset_y);
        delta_theta += -(delta_pos_in_vehicle[i].x - delta_pos_in_vehicle[i-1].x )/delta_offset_y; 
        omiga += -(pivot_vel_in_vehicle[i].x - pivot_vel_in_vehicle[i-1].x) / delta_offset_y;       
      }

      if ((i+1) == pivot_num)
      {
        if (abs(pivot_param[i].offset_x-pivot_param[0].offset_x) >= 
          abs(pivot_param[i].offset_y-pivot_param[0].offset_y))
        {
          delta_offset_x = (pivot_param[i].offset_x-pivot_param[0].offset_x);
          delta_theta += (delta_pos_in_vehicle[i].y - delta_pos_in_vehicle[0].y  )/delta_offset_x;
          omiga += (pivot_vel_in_vehicle[i].y - pivot_vel_in_vehicle[0].y) / delta_offset_x;
        }
        else 
        {
          delta_offset_y = (pivot_param[i].offset_y-pivot_param[0].offset_y);
          delta_theta += -(delta_pos_in_vehicle[i].x - delta_pos_in_vehicle[0].x  )/delta_offset_y;
          omiga += -(pivot_vel_in_vehicle[i].x - pivot_vel_in_vehicle[0].x) / delta_offset_y;        
        }
      }      
    }
    delta_theta /= pivot_num;
    omiga /= pivot_num;
    // RCLCPP_INFO_STREAM("delta_theta :" <<delta_theta);

    //step3  calculate delta x and delta y of vehicle
    double delta_pos_x = 0.0, delta_pos_y = 0.0 , vx =0.0 , vy = 0.0;
    Sxya delta_pos;
    delta_pos.x =0.0;
    delta_pos.y =0.0;
    delta_pos.a =delta_theta;

    for (int i =0 ;i<pivot_num ;i++)
    { 
      delta_pos.x += delta_pos_in_vehicle[i].x + pivot_param[i].offset_y * delta_theta;
      delta_pos.y += delta_pos_in_vehicle[i].y - pivot_param[i].offset_x * delta_theta;
      vx += pivot_vel_in_vehicle[i].x + pivot_param[i].offset_y * omiga;
      vy += pivot_vel_in_vehicle[i].y - pivot_param[i].offset_x * omiga;
    }
    delta_pos.x /= pivot_num;
    delta_pos.y /= pivot_num;
    vx /= pivot_num;
    vy /= pivot_num;

    //step4 accumulate position of vehicle
    if(standstill){
      delta_pos.x = 0.0;
      delta_pos.y = 0.0;
      delta_pos.a = 0.0;
    }
    
    pos_vehicle_.x += delta_pos.x;
    pos_vehicle_.y += delta_pos.y;
    pos_vehicle_.a += delta_pos.a;
    pos_vehicle_.a = normAngle2(pos_vehicle_.a);

    Sxya delta_pos_map;
    delta_pos_map = frameTrans(delta_pos, position_.a);
    position_.x += delta_pos_map.x;
    position_.y += delta_pos_map.y;
    position_.a += delta_pos_map.a;
    position_.a = normAngle2(position_.a);

    velocity_.x = vx;
    velocity_.y = vy;
    velocity_.a = omiga;
    DEBUG_OUT_THROTTLE(get_clock(),
      500,"[odom_multi_pivot] position  :" <<position_.x <<"," <<position_.y <<"," <<position_.a );
    DEBUG_OUT_THROTTLE(get_clock(),
      500,"[odom_multi_pivot] velocity  :" <<velocity_.x <<"," <<velocity_.y <<"," <<velocity_.a );

  }

  Sxya MultiPivotsOdometry::frameTrans(Sxya original , double angle)
  { 
    Sxya target;
    target.x = original.x * cos(angle) - original.y * sin(angle);
    target.y = original.x * sin(angle) + original.y * cos(angle);
    target.a = original.a;
    return target;
  }

} // namespace agv_tricycle_controller
